function collectivePF(N)
f=figure; clf;

% Load the environment
w=imread('autolabThick.png');
[dy,dx]=size(w);
image(w); hold on;
dy=dy-1;
dx=dx-1;
colormap gray;hold on;
% Create a variable with all the particles
P=zeros(N,4);
cnt=1;
% Initialise the particles
while (cnt<N)
    x=rand(1,1)*dx+1;
    y=rand(1,1)*dy+1;
    
    P(cnt,3)=rand(1,1)*2*pi;
    %If they fall inside a wall do not create
    if(w(int64(round(y)),int64(round(x)))~=0)
        cnt=cnt+1;
        P(cnt,1:2)=[x,y];
    end
end

P(:,4)=1/N;
plot(P(:,1),P(:,2),'r*');hold on;

%Initialise velosities
a=0.0;
V=2.5;

for t=1:900
    % Plot the environment at every step.
    image(w); hold on; % comment out for trails
    % add some noise to the linear velocity
    Vn=(V+randn(N,1)*0.2);
    % calculate position
    P(:,1)=P(:,1)+Vn.*cos(P(:,3));
    P(:,2)=P(:,2)+Vn.*sin(P(:,3));
    % calculate orientation with noisy angular velocity
    P(:,3)=P(:,3)+a+randn(N,1)*0.09;
    % check which particles are not good
    for k=1:N
        % check k particle
        x=int64(round(P(k,1)));
        y=int64(round(P(k,2)));
        
        if((x<=0)||(y<=0)) %if out of bounds
            P(k,4)=0; % Set the weight to zero
            plot(x,y,'bo');
        elseif((x>dx)||(y>dy)) %if out of bounds
            P(k,4)=0; % Set the weight to zero
            plot(x,y,'co');
        elseif(w(y,x)==0) % if inside a wall
            P(k,4)=0; % Set the weight to zero
            plot(x,y,'mo');
        end
    end
end

P(1:N,:)=resample1(P(1:N,:));

plot(P(:,1),P(:,2),'g.');
axis equal;
drawnow;
end
