Void’s Vault

Knowledge source for efficiency.

Particle Filter

Still reading the same book of Probabilistic Robotics, I implemented a Particle Filter. I show in this article my whole code to draw the filtering of a robot’s position. The problem used is the same used for implementing the unscented Kalman filter.

Enjoy!

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
clearvars;
clf;

N = 10;     %Number of iterations
M = 1000;   %Number of particules

SigmaX = 0.01; % Noise over x
SigmaY = 0.01; % Noise over y
SigmaZ = 0.001;  % Noise over z

GT = zeros(2,N); % Ground truth : the real position of the robot.
GT(:,1) = [1;1];

X = zeros(2,M,N); % Final belief matrix. Contains all the particules that estimates the real position.
x = [1;1];
X(:,:,1) = x(:,ones(M/2,numel(x)));
X_ = zeros(2,M); %Intermediate belief matrix. Contains the particules and their relative weights.
w = zeros(M);

for t = 2:1:N
    u1 = [3,2^(t/10)]; % The action taken by the robot.

    GT(1,t) = GT(1,t-1) + u1(1) + SigmaX*randn; %Non linear because of the Gaussian noise
    GT(2,t) = GT(2,t-1) + u1(2) + SigmaY*randn;

    z1 = 100/(sqrt(GT(1,t)^2 + GT(2,t)^2)) + SigmaZ*randn; %Non linear measurement function

    for m=1:1:M %We estimate the new position by creating particules
        X_(1,m) = X(1,m, t-1) +u1(1)+ SigmaX*randn;        %x
        X_(2,m) = X(2,m, t-1) +u1(2)+ SigmaY*randn;        %y
        X_(3,m) = abs(100/(sqrt(X_(1,m)^2 + X_(2,m)^2)) - z1); %w
    end

    %We invert the calculated weights now that we have all of them
    X_(3,:) = 1 - (X_(3,:)/max(X_(3,:))) + 0.0001;

    %We sum the weights for the upcoming particules draw.
    totalWeight = sum(X_(3,:));

    %We sort X_ for the upcoming particule draw. (optional because rand is uniformly distributed)
    %X_ = sortrows(X_',3)';

    for m=1:1:M
        w = rand*totalWeight; %Uniformely drawed pseudo-random number
        %We search the particule that have been selected.
        i = 1;
        w = w - X_(3,i);

        while w > 0
            i = i + 1;
            w = w - X_(3,i);
        end
        X(1,m,t) = X_(1,i); %We put the selected particule in the current belief
        X(2,m,t) = X_(2,i);
    end
end

MuA = zeros(2,N); %Mean of the particules
for t=1:1:N
    MuA(:,t) = mean(X(:,:,t),2);
end

h(1) = plot(GT(1,:),GT(2,:),'go');
hold on;
h(2) = plot(MuA(1,:),MuA(2,:),'b+');

That’s about it!