Void’s Vault

Knowledge source for efficiency.

Bayes Filter Algorithm

I was reading a book from Thrun, Burgard and Fox called “Probabilistic Robotics”, when I decided to implement some filtering algorithms found in the book. So here you will find the maltab script to implement a Bayes filter. I implemented this filter following the example 2.4.2, the mobile robot estimating the state of a door.

So the problem is : We have a robot that initially don’t know if a door is open or closed. First, he’ll try to sense the state of the door. Then he will push the door and use its sensors again. We use the Bayes filter to help the little robot to adjust its belief about the state of the door.

We initially define some constants:

1
2
3
4
is_open = 1;
is_closed = 0;
is_push = 1;
is_noop = 0;

Then, we set the probabilities about what should detect the sensor:

1
2
3
4
pz(1) = 0.8;    % p(Z_closed | X_closed)
pz(2) = 0.4;   % p(Z_closed | X_open)
pz(3) = 0.2;   % p(Z_open   | X_closed)
pz(4) = 0.6;   % p(Z_open   | X_open)

Then, we set the probabilities of the state of the door:

1
2
3
4
5
6
7
8
px((0*4) + (0*2) + (0*1) + 1) = 1;     % p(X_closed | U_noop, X_closed) = 1;
px((0*4) + (0*2) + (1*1) + 1) = 0;        % p(X_closed | U_noop, X_open) = 0;
px((0*4) + (1*2) + (0*1) + 1) = 0.2;   % p(X_closed | U_push, X_closed) = 0.2;
px((0*4) + (1*2) + (1*1) + 1) = 0;        % p(X_closed | U_push, X_open) = 0;
px((1*4) + (0*2) + (0*1) + 1) = 0;        % p(X_open   | U_noop, X_closed) = 0;
px((1*4) + (0*2) + (1*1) + 1) = 1;        % p(X_open   | U_noop, X_open) = 1;
px((1*4) + (1*2) + (0*1) + 1) = 0.8;   % p(X_open   | U_push, X_closed) = 0.8;
px((1*4) + (1*2) + (1*1) + 1) = 1;        % p(X_open   | U_push, X_open) = 1;

When that is done, we set the initial belief about the door state:

1
2
3
4
5
6
7
8
9
10
11
belief = zeros(2,3);
belief_before_z = zeros(2,3);

t = 1;
belief(is_closed+1, t) = 0.5;
belief(is_open+1, t) = 0.5;

belief_before_z(is_closed+1, t) = 0;    % Not used, if not to fill the first position of the table
belief_before_z(is_open+1, t) = 0;      % Not used, if not to fill the first position of the table
u(t) = 0;                             % Not used, if not to fill the first position of the table
z(t) = 0;                             % Not used, if not to fill the first position of the table

The first action is to do nothing and scan the door. so we update our belief this way:

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
t = 2
u(t) = is_noop;
z(t) = is_open; %We suppose here that the robot detected an open door

%belbefz(t) = sum(p(xt | ut , xt-1) * bel(xt-1)
belief_before_z(is_open+1, t) =   (px((is_open*4)   + (u(t)*2) + (is_open*1) + 1) * belief(is_open+1, t-1)) + (px((is_open*4)   + (u(t)*2) + (is_closed*1) + 1) * belief(is_closed+1, t-1));
belief_before_z(is_closed+1, t) = (px((is_closed*4) + (u(t)*2) + (is_open*1) + 1) * belief(is_open+1, t-1)) + (px((is_closed*4) + (u(t)*2) + (is_closed*1) + 1) * belief(is_closed+1, t-1))

%belief(t) = n * p(zt | xt) * belbefz(t)
belief(is_open+1, t)   = pz((z(t)*2) + (is_open*1) + 1) * belief_before_z(is_open+1, t);
belief(is_closed+1, t) = pz((z(t)*2) + (is_closed*1) + 1) * belief_before_z(is_closed+1, t);

%normalisation de belief
normalizer = 1 / (belief(is_open+1, t) + belief(is_closed+1, t))
belief(is_open+1, t) = belief(is_open+1, t) * normalizer;
belief(is_closed+1, t) = belief(is_closed+1, t) * normalizer

Then, we push the door and suppose again that the robot detects an open door:

1
2
3
4
5
6
7
8
9
10
11
12
u(t) = is_push;
z(t) = is_open;

belief_before_z(is_open+1, t) = (px((is_open*4) + (u(t)*2) + (is_open*1) + 1) * belief(is_open+1, t-1)) + (px((is_open*4) + (u(t)*2) + (is_closed*1) + 1) * belief(is_closed+1, t-1));
belief_before_z(is_closed+1, t) = (px((is_closed*4) + (u(t)*2) + (is_open*1) + 1) * belief(is_open+1, t-1)) + (px((is_closed*4) + (u(t)*2) + (is_closed*1) + 1) * belief(is_closed+1, t-1))

belief(is_open+1, t)   = pz((z(t)*2) + (is_open*1) + 1) * belief_before_z(is_open+1, t);
belief(is_closed+1, t) = pz((z(t)*2) + (is_closed*1) + 1) * belief_before_z(is_closed+1, t);

normalizer = 1 / (belief(is_open+1, t) + belief(is_closed+1, t))
belief(is_open+1, t) = belief(is_open+1, t) * normalizer;
belief(is_closed+1, t) = belief(is_closed+1, t) * normalizer

And that’s it, we have about 98.3% of probability of having and open door. But be careful to still consider that the door still might be closed, or you might crash on it!