Kalman Filter(2) — Grid World Localisation

Kalman Filter(2) — Grid World Localisation

Apply Basics to 2 Dimensional Space

In lastpost, we have applied basic Bayes rules and total probability to localise a moving car in a 1-dimension world. Let’s reinforce our understanding and apply them to a 2-dimension world.

Problem Setting

Consider a 2 dimensional world, the robot can move only left, right, up, or down. It cannot move diagonally. Also, for this assignment, the robot will never overshoot its destination square; it will either make the movement or it will remain stationary.

And for each movement, it follows:

And the measurements and motions are set to:

The setting says in this 3x3 world, there are 2 different colours — green and red. The robot starts with equal probability in each cell and a measurement of R , with the sensor has a probability of 0.8 being correct. Then the robot takes a motion [0, 0] , which is to stay still and the motion has a probability of 1.0 being executed correctly. The question is what is current probability of the robot being in each cell?

Implementation

Initialisation

Initially, each cell has equal probability:

Sense

Based on the sensor’s measurement(measurement probability), and initial probability(prior probability), applying Bayes rules, we can get the posterior probability.

Move

In movement, we apply total probability,

that is the probability of each cell equals the multiplication of the probability of the cell it comes from and transition probability.

The situation is simpler here, as the robot can only take the move or stay stationary( (i, j) is the current cell, (i-U[0], j-U[1]) would be the cell it moves from).

Localisation

Now let’s assemble the parts:

And correspondingly we get the result:

[[0.06666667 0.06666667 0.06666667]
 [0.06666667 0.26666667 0.26666667]
 [0.06666667 0.06666667 0.06666667]]

so after the measurement and action, the robot is most likely to stay in cell (1, 1) or (1, 2) .

For more complete tests, please check full implementation here .

Reference:

1. https://classroom.udacity.com/courses/cs373